clc
clear

theta = deg2rad(30);  
R_AB = [cos(theta) -sin(theta) 0; sin(theta) cos(theta) 0; 0 0 1];
PB_ORG = [4 3 0]';
homogenization = [0 0 0 1];
T = [R_AB PB_ORG];
T_AB = [T;homogenization];
fprintf("Homeogenous Transformation Matrix:\n");
disp(T);

fprintf("Homeogenous Inverse Transformation Matrix:\n");
T_BA = [R_AB' -R_AB'*PB_ORG];
T_BA = [T_BA;homogenization];
disp(T_BA);